#include <BulletClassDataSource/RigidBodyBullet.h>
#include <osg/Node>
#include <iostream>
#include <EDesignerCore.h>
#include <QDateTime>

#include <EngineFunctions.h>
#include <PhysicsWorldFunctions.h>
#include <PhysicsWorld.h>
#include <FindNodeByNameVisitor.h>
#include <SceneViewer.h>
#include <SceneViewerFunctions.h>
#include <btBulletDynamicsCommon.h>

#include <SDFManager.h>

#include <osg/ShapeDrawable>
#include <osgbDynamics/MotionState.h>
#include <SDFDofRange.h>

#include <SDFSprayAlgorithm.h>
RigidBodyBullet::RigidBodyBullet( QObject *parent /*= 0*/ ) : CollisionObjectBullet(parent)
{
    refreshItemTest();
    refreshInfoContainer();

    //btRigidBody* rigidBody = new btRigidBody();
    m_collisionShapeType = COLLISION_SHAPE::BOX_SHAPE_PROXYTYPE;
    m_mass = 0;
    m_transformNodeName = "RigidBodyBullet";

    m_sdfPoint3DNode = new Point3D;
    osg::Matrix m = m_sdfPoint3DNode->getMatrix();
    m_sdfPoint3DNode->setRadius(0.001);
    m_sdfPoint3DNode->setCoordinateSize(2);
    osg::Vec3d trans = m.getTrans();
    m_sdfPointMatrix(0,3) = trans.x();
    m_sdfPointMatrix(1,3) = trans.y();
    m_sdfPointMatrix(2,3) = trans.z();

    osg::Vec3d rotate = SceneViewerFunctions::osg_matrix_getRotate(m);
    m_sdfPointMatrix(0,2) = rotate.x();
    m_sdfPointMatrix(1,2) = rotate.y();
    m_sdfPointMatrix(2,2) = rotate.z();

    osg::Vec3d scale = m.getScale();
    m_sdfPointMatrix(0,1) = scale.x();
    m_sdfPointMatrix(1,1) = scale.y();
    m_sdfPointMatrix(2,1) = scale.z();

    m_dofTest.setX(1);
    m_dofTest.setY(1);
    m_dofTest.setZ(1);
}

RigidBodyBullet::~RigidBodyBullet()
{
    btRigidBody* rigidBody = dynamic_cast<btRigidBody*>(m_collisioinObj);
    if(rigidBody != NULL)
    {
        PhysicsWorld::instance()->removeRigidBody(rigidBody);
        //delete rigidBody;
    }
}

void RigidBodyBullet::setTransformNodeName(const QString &name)
{
    if(m_transformNodeName != name)
    {
        m_transformNodeName = name;
        refreshItemTest();
        rebuildRigidBody();

    }
    m_transformNodeName = name;
}

QString RigidBodyBullet::getTransformNodeName()
{
    return m_transformNodeName;
}

RigidBodyBullet::COLLISION_SHAPE RigidBodyBullet::getCollisionShapeType()
{
    return m_collisionShapeType;
}

void RigidBodyBullet::setCollisionShapeType(COLLISION_SHAPE shapeType)
{
    if(m_collisionShapeType != shapeType)
    {
        m_collisionShapeType = shapeType;
        rebuildRigidBody();
    }

    m_collisionShapeType = shapeType;

}

double RigidBodyBullet::getMass()
{
    return m_mass;
}

void RigidBodyBullet::setMass(double mass)
{
    if(m_mass != mass)
    {
        m_mass = mass;
        //std::cout<<"RigidBodyBullet::setMass"<<std::endl;
        rebuildRigidBody();
    }

    m_mass = mass;
}

QVarintTrigger RigidBodyBullet::getTriggerRebuild()
{
    return QVarintTrigger();
}

void RigidBodyBullet::setTriggerRebuild(QVarintTrigger trigger)
{
    rebuildRigidBody();
}

QVector3D RigidBodyBullet::getGridIndex()
{
    return m_gridIndex;
}

void RigidBodyBullet::setGridIndex(QVector3D gridIndex)
{
    m_gridIndex = gridIndex;   
    SDF* sdfPtr = SDFManager::instance()->getSDF(SDFManager::instance()->getPtr((int)m_collisioinObj));

    if(sdfPtr == NULL)
        return;

    int m_I = sdfPtr->getNodeCountVector().x();
    int m_J = sdfPtr->getNodeCountVector().y();
    int m_K = sdfPtr->getNodeCountVector().z();

    if(m_gridIndex.x() < 0 )
        m_gridIndex.setX(0);
    if(m_gridIndex.y() < 0)
        m_gridIndex.setY(0);
    if(m_gridIndex.z() < 0)
        m_gridIndex.setZ(0);

    if(m_gridIndex.x() >= m_I )
        m_gridIndex.setX(m_I-1);
    if(m_gridIndex.y() >= m_J)
        m_gridIndex.setY(m_J-1);
    if(m_gridIndex.z() >= m_K)
        m_gridIndex.setZ(m_K-1);


    float gridValue = sdfPtr->getGridValueByIJK(m_gridIndex.x(), m_gridIndex.y(), m_gridIndex.z());
    std::cout<<"RigidBodyBullet::setGridIndex value="<<gridValue<<std::endl;
    //std::cout<<"RigidBodyBullet::setGridIndex index="<<m_gridIndex.x()<<","<<m_gridIndex.y()<<","<<m_gridIndex.z()<<std::endl;
    //std::cout<<"RigidBodyBullet::setGridIndex isClosed="<<sdfPtr->isPointClosedByIJK3(m_gridIndex.x(), m_gridIndex.y(), m_gridIndex.z())<<std::endl;

    osg::Vec3 pos = sdfPtr->getPositionInSDFFrameByIJK(m_gridIndex.x(), m_gridIndex.y(), m_gridIndex.z());
    osg::Vec3 gradient = sdfPtr->getGradientByIJK(m_gridIndex.x(), m_gridIndex.y(), m_gridIndex.z());

    osg::Matrix tm;
    tm.setTrans(pos.x(), pos.y(), pos.z());
    osg::Matrix rm;
    rm.makeRotate(osg::Vec3(1,0,0), gradient);

    m_sdfPoint3DNode->setMatrix(rm*tm);
}

double RigidBodyBullet::getGridSphereRaidus()
{
    return m_sdfPoint3DNode->getRadius();
}

void RigidBodyBullet::setGridSphereRaidus(double doubleValue)
{
    doubleValue = 0.001;
    m_sdfPoint3DNode->setRadius(doubleValue);
    m_sdfPoint3DNode->setCoordinateSize(2);//3*doubleValue
}

QMatrix4x4 RigidBodyBullet::getSDFSampleMatrix()
{
    //QMatrix m_sdfPointMatrix;

    osg::Matrix m = m_sdfPoint3DNode->getMatrix();
    osg::Vec3d trans = m.getTrans();
    m_sdfPointMatrix(0,3) = trans.x();
    m_sdfPointMatrix(1,3) = trans.y();
    m_sdfPointMatrix(2,3) = trans.z();

    osg::Vec3d rotate = SceneViewerFunctions::osg_matrix_getRotate(m);
    m_sdfPointMatrix(0,2) = rotate.x();
    m_sdfPointMatrix(1,2) = rotate.y();
    m_sdfPointMatrix(2,2) = rotate.z();

    osg::Vec3d scale = m.getScale();
    m_sdfPointMatrix(0,1) = scale.x();
    m_sdfPointMatrix(1,1) = scale.y();
    m_sdfPointMatrix(2,1) = scale.z();

    return m_sdfPointMatrix;
}

void RigidBodyBullet::setSDFSampleMatrix(QMatrix4x4 matrix)
{
    osg::Matrix m = SceneViewerFunctions::osg_matrix_ScaleRotateTrans(osg::Vec3(matrix(0,1),matrix(1,1), matrix(2,1)),
                                                                      osg::Vec3(matrix(0,2),matrix(1,2), matrix(2,2)),
                                                                      osg::Vec3(matrix(0,3),matrix(1,3), matrix(2,3)));



    m_sdfPoint3DNode->setMatrix(m);
    m_sdfPointMatrix = matrix;

}

QVector3D RigidBodyBullet::getSDFSamplePosition()
{
    QVector3D position;
    osg::Matrix m = m_sdfPoint3DNode->getMatrix();
    osg::Vec3d trans = m.getTrans();
    position.setX(trans.x());
    position.setY(trans.y());
    position.setZ(trans.z());

    return position;

}

void RigidBodyBullet::setSDFSamplePosition(QVector3D position)
{

    SDF* sdfPtr = SDFManager::instance()->getSDF(SDFManager::instance()->getPtr((int)m_collisioinObj));
    if(sdfPtr == NULL)
        return;
    osg::Vec3 sdfMaxCoordinate = sdfPtr->getMaxCoordinate();
    osg::Vec3 sdfMinCoordinate = sdfPtr->getMinCoordinate();
    double pos_x = qMin((float)sdfMaxCoordinate.x(), (float)position.x());
    double pos_y = qMin((float)sdfMaxCoordinate.y(), (float)position.y());
    double pos_z = qMin((float)sdfMaxCoordinate.z(), (float)position.z());
    pos_x = qMax((float)sdfMinCoordinate.x(),(float)pos_x);
    pos_y = qMax((float)sdfMinCoordinate.y(),(float)pos_y);
    pos_z = qMax((float)sdfMinCoordinate.z(),(float)pos_z);

    //osg::Matrix m = m_sdfPoint3DNode->getMatrix();
    //m.setTrans(pos_x, pos_y, pos_z);
    //m_sdfPoint3DNode->setMatrix(m);

    //print enclosing index
    //int i0,i1,j0,j1,k0,k1;
    //sdfPtr->enclosing_indices(osg::Vec3(pos_x, pos_y, pos_z),i0, j0, k0, i1, j1, k1);
    //std::cout<<"RigidBodyBullet::setSDFSamplePosition ijk="<<i0<<","<<j0<<","<<k0<<","<<i1<<","<<j1<<","<<k1<<std::endl;
    float gridValue = sdfPtr->getGridValueByPosition(pos_x, pos_y, pos_z);
    std::cout<<"RigidBodyBullet::setSDFSamplePosition value="<<gridValue<<std::endl;
    //bool boolValue = sdfPtr->isPointNearSharpEdge(pos_x, pos_y, pos_z);
    //std::cout<<"RigidBodyBullet::setSDFSamplePosition isNearEdge="<<boolValue<<std::endl;

    osg::Vec3 gradient = sdfPtr->getGradientByPosition(pos_x, pos_y, pos_z);

    osg::Matrix tm;
    tm.setTrans(pos_x, pos_y, pos_z);
    osg::Matrix rm;
    rm.makeRotate(osg::Vec3(1,0,0), gradient);

    m_sdfPoint3DNode->setMatrix(rm*tm);


}

QVarintTrigger RigidBodyBullet::getTriggerDOFCompute()
{
    return QVarintTrigger();
}

void RigidBodyBullet::setTriggerDOFCompute(QVarintTrigger trigger)
{
    //SDFManager::instance()->DofCompute(SDFManager::instance()->getPtr((int)m_collisioinObj));

    SDFManager::instance()->computeSDFDOf(
                SDFManager::instance()->getSDF(SDFManager::instance()->getPtr((int)m_collisioinObj))
                );

    SDFManager::instance()->saveContactsToMatlabInputFile(
                SDFManager::instance()->getSDF(SDFManager::instance()->getPtr((int)m_collisioinObj))
                );
}

QVector3D RigidBodyBullet::getDofTest()
{
    return m_dofTest;
}

void RigidBodyBullet::setDofTest(QVector3D position)
{
    m_dofTest = position;
    osg::Vec3 n(position.x(), position.y(), position.z());
    SDFDofRange range;
    range.computeRangeFromNormal(n);
    range.print();
}

QVarintTrigger RigidBodyBullet::getTriggerSprayCompute()
{
    return QVarintTrigger();
}

void RigidBodyBullet::setTriggerSprayCompute(QVarintTrigger trigger)
{
    SDFSprayAlgorithm::instance()->generatePoints(
    SDFManager::instance()->getSDF(SDFManager::instance()->getPtr((int)m_collisioinObj))
                );

}



btRigidBody *RigidBodyBullet::getRigidBody()
{
    btRigidBody* rigidBody = dynamic_cast<btRigidBody*>(m_collisioinObj);
    return rigidBody;
}


void RigidBodyBullet::rebuildRigidBody()
{
    //first remove rigidBody
    btRigidBody* rigidBody = dynamic_cast<btRigidBody*>(m_collisioinObj);
    if(rigidBody != NULL)
    {
        PhysicsWorld::instance()->removeRigidBody(rigidBody);
    }


    //second find the node with transformNodeName
    osg::ref_ptr<osg::Node> findedNode = SceneViewerFunctions::findNodeByName(m_transformNodeName.toStdString());
    //std::cout<<"RigidBodyBullet::rebuildRigidBody findedNode = "<<(int)findedNode.get()<<std::endl;
    if(findedNode.get() != NULL)
    {
        if(m_sdfPoint3DNode->getParents().size() > 0)
        {
            osg::Group* partentPtr = m_sdfPoint3DNode->getParent(0);
            partentPtr->removeChild(m_sdfPoint3DNode.get());
        }


        rigidBody = PhysicsWorldFunctions::creatRigidBodyFromOsgNode(findedNode, m_mass, (PhysicsWorldFunctions::COLLISION_SHAPE)m_collisionShapeType);
        setCollisionObj(rigidBody);

        rebuildSDFSphereNode();
        //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step8"<<std::endl;

    }

    refreshInfoContainer();

}

void RigidBodyBullet::refreshItemTest()
{
    setEObjectName(this->metaObject()->className() + QString("-") + m_transformNodeName);
}

void RigidBodyBullet::rebuildSDFSphereNode()
{
    //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step1"<<std::endl;
    m_sdfPoint3DNode->setRadius(m_sdfPoint3DNode->getRadius());
    //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step2"<<std::endl;
    if(m_sdfPoint3DNode->getParents().size() > 0)
    {
        //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step3"<<std::endl;

        osg::Group* partentPtr = m_sdfPoint3DNode->getParent(0);
        partentPtr->removeChild(m_sdfPoint3DNode.get());
    }

    btRigidBody* rigidBody = dynamic_cast<btRigidBody*>(m_collisioinObj);
    //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step4"<<std::endl;

    if(rigidBody != NULL)
    {
        //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step5"<<std::endl;

        osgbDynamics::MotionState * motion = dynamic_cast<osgbDynamics::MotionState *>(rigidBody->getMotionState());
        if(motion != NULL && motion->getTransform() != NULL)
        {
            //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step6"<<std::endl;

            motion->getTransform()->addChild(m_sdfPoint3DNode);
        }
    }
    //std::cout<<"RigidBodyBullet::rebuildSDFSphereNode step7"<<std::endl;

}
